using System;
using System.Diagnostics;

namespace Dida
{
	/// <summary>
	/// This class contains the result values of a collision test.
	/// </summary>
	/// <seealso cref="CollisionTriangle"/>
	public class CollisionResult
	{
		/// <summary>
		/// The relative distance of the collision, relative to the
		/// path of the collision test. When the collision occurs at 
		/// the beginning of the path, it will be 0, and the closer
		/// it comes to the end of the path the closer the value will
		/// come to 1.
		/// If you want to calculate the position of the actual collision,
		/// you just add the motion multiplied by this distance to the
		/// initial position:
		/// <example>
		/// Vector3D collisionPos = start + motion * collisionResult.dist;
		/// </example>
		/// </summary>
		public float dist;
		public Vector3D normal;
	}

	public partial class CollisionTriangle
	{
		public bool TestForCollision(Sphere sphere, Vector3D dir,
			out CollisionResult res)
		{
			Sphere pathBSphere = new Sphere(
				sphere.center + dir * .5f,
				sphere.radius + dir.Len * .5f);
			return TestForCollision(sphere, dir, out res, pathBSphere);
		}

		public bool TestForCollision(Sphere sphere, Vector3D dir,
			out CollisionResult res, Sphere pathBSphere)
		{
			res = null;

			if (EarlyRejection(pathBSphere))
				return false;

			IntermediateResult planeRes = TestPlane(sphere, dir, ref res);
			if (planeRes == IntermediateResult.NoCollision)
				return false;
			else if (planeRes == IntermediateResult.Collision)
				return true;

			bool[] vertexNeedsTest = new bool[] { true, true, true };

			// test the edges
			for (int i = 0; i < 3; i++)
			{
				CollisionResult curRes;
				IntermediateResult edgeRes = TestEdge(i, sphere, dir, out curRes);
				if (edgeRes != IntermediateResult.DontKnowYet)
				{
					vertexNeedsTest[i] = false;
					vertexNeedsTest[(i + 1) % 3] = false;
				}

				if (edgeRes == IntermediateResult.Collision)
				{
					if(res == null)
						res = curRes;
					else if (curRes.dist < res.dist)
						res = curRes;
				}
			}

			// test the vertices
			for (int i = 0; i < 3; i++)
			{
				if (vertexNeedsTest[i])
				{
					CollisionResult curRes;
					if (TestVertex(i, sphere, dir, out curRes))
					{
						if (res == null)
							res = curRes;
						else if (curRes.dist < res.dist)
							res = curRes;							
					}
				}
			}

			if (res != null)
				Debug.Assert(res.dist < 1.001f);

			return res != null;
		}

		private bool EarlyRejection(Sphere pathBSphere)
		{
			float facePlaneDist = facePlane.GetDist(pathBSphere.center);
			return facePlaneDist < -pathBSphere.radius ||
				facePlaneDist > pathBSphere.radius ||
				edgePlanes[0].GetDist(pathBSphere.center) < -pathBSphere.radius ||
				edgePlanes[1].GetDist(pathBSphere.center) < -pathBSphere.radius ||
				edgePlanes[2].GetDist(pathBSphere.center) < -pathBSphere.radius;
		}

		private IntermediateResult TestPlane(Sphere sphere,Vector3D dir,
			ref CollisionResult res)
		{
			float startDist = facePlane.GetDist(sphere.center);
			float endDist = startDist + Vector3D.DotProduct(
				facePlane.normal, dir);

			if (startDist > sphere.radius && endDist > sphere.radius)
				return IntermediateResult.NoCollision;
			if (startDist < -sphere.radius && endDist < -sphere.radius)
				return IntermediateResult.NoCollision;

			if (startDist > sphere.radius)
			{
				if (endDist >= startDist)
					return IntermediateResult.NoCollision;

				float onPlaneDist = (sphere.radius - startDist) / (endDist - startDist);
				Vector3D onPlane = sphere.center + dir * onPlaneDist;
				if (PointInTriangle(onPlane))
				{
					res = new CollisionResult();
					res.normal = facePlane.normal;
					res.dist = onPlaneDist;
					return IntermediateResult.Collision;
				}
				else
					return IntermediateResult.DontKnowYet;
			}
			else if (startDist < -sphere.radius)
			{
				if (endDist <= startDist)
					return IntermediateResult.NoCollision;

				float onPlaneDist = (-sphere.radius - startDist) / (endDist - startDist);
				Vector3D onPlane = sphere.center + dir * onPlaneDist;
				if (PointInTriangle(onPlane))
				{
					res = new CollisionResult();
					res.normal = -facePlane.normal;
					res.dist = onPlaneDist;
					return IntermediateResult.Collision;
				}
				else
					return IntermediateResult.DontKnowYet;
			}
			else if (startDist > 0)
			{
				if (endDist < startDist)
				{
					// the sphere was already inside the plane, and tries to
					// move even deeper.
					if (PointInTriangle(sphere.center))
					{
						res = new CollisionResult();
						res.dist = 0;
						res.normal = facePlane.normal;
						return IntermediateResult.Collision;
					}
					else
					{
						// the sphere's center isn't on the actual triangle,
						// so we have to test the edges and vertices.
						return IntermediateResult.DontKnowYet;
					}
				}
				else
				{
					// the sphere was already inside the plane, but tries
					// to move out, so we let it go.
					if (PointInTriangle(sphere.center))
						return IntermediateResult.NoCollision;
					else
					{
						// the sphere's center isn't on the actual triangle,
						// so we have to test the edges and vertices.
						return IntermediateResult.DontKnowYet;
					}
				}
			}
			else // if(startDist <= 0)
			{
				if (endDist > startDist)
				{
					// the sphere was already inside the plane, and tries to
					// move even deeper.
					if (PointInTriangle(sphere.center))
					{
						res = new CollisionResult();
						res.dist = 0;
						res.normal = -facePlane.normal;
						return IntermediateResult.Collision;
					}
					else
					{
						// the sphere's center isn't on the actual triangle,
						// so we have to test the edges and vertices.
						return IntermediateResult.DontKnowYet;
					}
				}
				else
				{
					// the sphere was already inside the plane, but tries
					// to move out, so we let it go.
					if (PointInTriangle(sphere.center))
						return IntermediateResult.NoCollision;
					else
					{
						// the sphere's center isn't on the actual triangle,
						// so we have to test the edges and vertices.
						return IntermediateResult.DontKnowYet;
					}
				}
			}
		}

		/// <summary>
		/// <para>
		/// Projects a point to the edge space of the edge with index i.
		/// </para>
		/// <para>
		/// Edge space is an orthogonal projection of the scene,
		/// viewed from the direction where the edge looks like a single point.
		/// (the edge is used as lookat vector).
		/// </para>
		/// </summary>
		/// <param name="i">The index of the edge to who's space
		/// we want to project.</param>
		/// <param name="pt">The point that has to be projected.</param>
		/// <returns>The result of the projection.</returns>
		private Vector2D ProjectToEdgeSpace(int i, Vector3D pt)
		{
			Vector2D ret = new Vector2D();
			ret.x = Vector3D.DotProduct(edgePlanes[i].normal, pt);
			ret.y = Vector3D.DotProduct(facePlane.normal, pt);
			return ret;
		}

		private IntermediateResult TestEdge(int i, Sphere sphere,Vector3D dir,
			out CollisionResult res)
		{
			Vector2D edge2D = ProjectToEdgeSpace(i, verts[i]);
			Vector2D sphereCenter2D = ProjectToEdgeSpace(i, sphere.center);
			Vector2D dir2D = ProjectToEdgeSpace(i, dir);

			float dirSqrLen = dir2D.SqrLen;
			Vector2D invDir = dir2D / dirSqrLen;

			float closestPtParam = Vector2D.DotProduct(edge2D - sphereCenter2D, invDir);
			Vector2D closestPt = sphereCenter2D + dir2D * closestPtParam;
			float closestPtSqrDist = (closestPt - edge2D).SqrLen;

			float sqrRad = sphere.radius * sphere.radius;

			if (closestPtSqrDist > sqrRad)
			{
				// the sphere doesn't come close enough to the infinite edge,
				// so there can't be a collision with this edge.
				res = null;
				return IntermediateResult.NoCollision;
			}

			float onEdgeOffset = (float)Math.Sqrt(sqrRad - closestPtSqrDist) /
				(float)Math.Sqrt(dirSqrLen);

			float onEdgeParam1 = closestPtParam - onEdgeOffset;
			//float onEdgeParam2 = closestPtParam + onEdgeOffset;

			if (1 < onEdgeParam1)
			{
				// the end of the path isn't even inside the edge.
				res = null;
				return IntermediateResult.DontKnowYet;
			}
			else if (0 < onEdgeParam1)
			{
				// the start of the path is before the edge

				Vector3D collisionPt = sphere.center + dir * onEdgeParam1;
				float onEdgePos = Vector3D.DotProduct(collisionPt - verts[i], invEdgeVecs[i]);
				if (onEdgePos > 0 && onEdgePos < 1)
				{
					// the collision is actually on the edge's segment

					Vector2D collisionPt2D = sphereCenter2D + dir2D * onEdgeParam1;
					Vector2D normal2D = collisionPt2D - edge2D;

					res = new CollisionResult();
					res.normal = edgePlanes[i].normal * normal2D.x +
						facePlane.normal * normal2D.y;
					res.normal.Normalize();
					res.dist = onEdgeParam1;
					return IntermediateResult.Collision;
				}
				else
				{
					// the sphere intersects the infinite edge, but isn't
					// on the actual segment. It may still collide with 
					// the vertex on this edge though, so we return: don't know yet
					res = null;
					return IntermediateResult.DontKnowYet;
				}
			}
			else if (0 < closestPtParam)
			{
				// the start of the path is already inside the edge,
				// and it tries to go even deeper, so we'll report a collision.

				float onEdgePos = Vector3D.DotProduct(sphere.center - verts[i], invEdgeVecs[i]);
				if (onEdgePos > 0 && onEdgePos < 1)
				{
					// the collision is actually on the edge's segment

					Vector2D normal2D = sphereCenter2D - edge2D;

					res = new CollisionResult();
					res.normal = edgePlanes[i].normal * normal2D.x +
						facePlane.normal * normal2D.y;
					res.normal.Normalize();
					res.dist = 0;
					return IntermediateResult.Collision;
				}
				else
				{
					// the sphere intersects the infinite edge, but isn't
					// on the actual segment. It may still collide with 
					// the vertex on this edge though, so we return: don't know yet
					res = null;
					return IntermediateResult.DontKnowYet;
				}
			}
			else
			{
				// the whole path is past the edge's middle,
				// so that means there's either no collision, or it was
				// already inside the edge, but is moving out, 
				// so we'll return no collison
				res = null;
				return IntermediateResult.NoCollision;
			}
		}

		private bool TestVertex(int i,Sphere sphere,Vector3D dir,
			out CollisionResult res)
		{
			// the dir with length 1 / dir.Len
			float dirSqrLen = dir.SqrLen;
			Vector3D invDir = dir / dirSqrLen;

			float closestPtParam = Vector3D.DotProduct(verts[i] - sphere.center, invDir);
			Vector3D closestPt = sphere.center + dir * closestPtParam;
			float closestPtSqrDist = (closestPt - verts[i]).SqrLen;

			float sqrRad = sphere.radius * sphere.radius;

			if (closestPtSqrDist > sqrRad)
			{
				res = null;
				return false;
			}

			float onSphereOffset = (float)Math.Sqrt(sqrRad - closestPtSqrDist) /
				(float)Math.Sqrt(dirSqrLen);

			float onSphereParam1 = closestPtParam - onSphereOffset;
			// float onSphereParam2 = closestPtParam + onSphereOffset;

			if (1 < onSphereParam1)
			{
				res = null;
				return false;
			}
			if (0 < onSphereParam1)
			{
				// the start of the path is before the vertex' sphere

				Vector3D collisionPt = sphere.center + dir * onSphereParam1;
				
				res = new CollisionResult();
				res.normal = collisionPt - verts[i];
				res.normal.Normalize();
				res.dist = onSphereParam1;
				return true;
			}
			else if (0 < closestPtParam)
			{
				// the start of the path is already inside the vertex' sphere,
				// and it tries to go even deeper, so we'll report a collision.

				res = new CollisionResult();
				res.normal = sphere.center - verts[i];
				res.normal.Normalize();
				res.dist = 0;
				return true;
			}
			else
			{
				// the whole path is past the vertex' sphere's middle,
				// so that means there's either no collision, or it was
				// already inside the vertex' sphere, but is moving out, 
				// so we'll return no collison
				res = null;
				return false;
			}
		}

		private enum IntermediateResult
		{
			NoCollision,
			Collision,
			DontKnowYet,
		}
	}
}